/*
 * ThreadDriveController.cpp
 *
 *  Created on: Jan 26, 2013
 *      Author: Drew Prevost
 */

#include "ThreadDriveController.h"

#define DEGREE_CLUTCH_DOWN 0
#define RATE_CLUTCH_DOWN 60
#define DEGREE_CLUTCH_MIDDLE 90
#define DEGREE_CLUTCH_UP 180
#define RATE_CLUTCH_UP 0

#define SPEED_DRIVE_HIGH 255
#define ACC_DRIVE_UP_TO_HIGH 7843
#define SPEED_DRIVED_OFF 0
#define DEC_DRIVE_OFF 0
#define SPEED_DRIVE_LOW 20
#define ACC_DRIVE_UP_TO_LOW 100000

#define SECONDS_FULL_THROTTLE 5

#define LOAD_WHEEL_MIN_RPM 50

ThreadDriveController::ThreadDriveController(
                                              int nTachPin,
                                              int nTachIrqNum,
                                              int nTachIsrPin,
                                              int nTachIsrMode,
                                              int nMotorPin,
                                              int nServoPin,
                                              unsigned long nServoMaxSpeedInMicrosPerDegree,
                                              unsigned int nServoInitialPosition,
                                              unsigned int nServoInitialRateInDegPerSec )
    : tachWheel( Tachometer::singleton() ),
      threadMotorDrive( nMotorPin ),
      threadServoClutch( nServoPin, nServoMaxSpeedInMicrosPerDegree, nServoInitialPosition, nServoInitialRateInDegPerSec )
{
  tachWheel.init( nTachIrqNum, nTachIsrPin, nTachIsrMode );
}

ThreadDriveController::~ThreadDriveController()
{
}

bool ThreadDriveController::run()
{
  threadMotorDrive.run();
  threadServoClutch.run();
  threadMicroDelay.run();

  PT_BEGIN()
  ;

// Step one: move clutch down until driveshaft begins to touch load wheel
//  lcd.clear();
//  lcd.print( "Step One" );
  threadServoClutch.setPosition( DEGREE_CLUTCH_DOWN, RATE_CLUTCH_DOWN );
// TODO: try <=
  PT_WAIT_UNTIL( threadServoClutch.getPosition() == DEGREE_CLUTCH_MIDDLE );

// Step two: begin to speed up motor while continuing to lower clutch
//  lcd.clear();
//  lcd.print( "Step Two" );
  threadMotorDrive.setSpeed( SPEED_DRIVE_HIGH, ACC_DRIVE_UP_TO_HIGH );
// TODO: try >=
  PT_WAIT_UNTIL( threadMotorDrive.getSpeed() == SPEED_DRIVE_HIGH );

// Step three: wait for a bit
//nHybridMode = HYBRID_MODE_HOLD_SPEED; // TODO: deal with this...
//  lcd.clear();
//  lcd.print( "Step Three" );
//tsVoltageMonitorBattery.bCheckedThisCoast = false;  // TODO: deal with this
  threadMicroDelay.setDelay( SECONDS_FULL_THROTTLE * 1000000 );
  PT_SPAWN( threadMicroDelay );

// Step four: raise clutch and turn off motor
//  lcd.clear();
//  lcd.print( "Step Four" );
  threadServoClutch.setPosition( DEGREE_CLUTCH_UP, RATE_CLUTCH_UP );
  PT_WAIT_UNTIL( threadServoClutch.getPosition() == DEGREE_CLUTCH_MIDDLE );
  threadMotorDrive.setSpeed( SPEED_DRIVED_OFF, DEC_DRIVE_OFF );

// Step five: wait until load wheel slows down
//nHybridMode = HYBRID_MODE_COAST;    // TODO: deal with this...
//  lcd.clear();
//  lcd.print( "Step Five" );
  PT_WAIT_UNTIL( tachWheel.getRpm() <= LOAD_WHEEL_MIN_RPM );

// Step six: raise motor speed to SPEED_DRIVE_LOW
//nHybridMode = HYBRID_MODE_ACCELERATE;      // TODO: deal with this...
  threadMotorDrive.setSpeed( SPEED_DRIVE_LOW, ACC_DRIVE_UP_TO_LOW );
// TODO: try >=
  PT_WAIT_UNTIL( threadMotorDrive.getSpeed() == SPEED_DRIVE_LOW );

PT_END()
;
return false;
}
